{
  "cells": [
    {
      "execution_count": null,
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      },
      "source": [
        "%matplotlib inline"
      ]
    },
    {
      "source": [
        "\n********************************************************************************\n2D Robot SLAM - Benchmark\n********************************************************************************\nGoals of this script:\n\n* implement three different UKFs on the 2D robot SLAM problem.\n\n*  (re)discover computational alternatives for performing UKF:\n    \n    * when Jacobian are (partially) known.\n    \n    * or when only a part of the state is involved in a propagation step.\n   \n    * or when only a part of the state is involved in a measurement.\n\n* design the Extended Kalman Filter (EKF) and the Invariant Extended Kalman\n  Filter (IEKF) :cite:`barrauInvariant2017`.\n\n* compare the different algorithms with Monte-Carlo simulations.\n\n*We assume the reader is already familiar with the considered problem described\nin the related example.*\n\nFor the given, three different UKFs emerge, defined respectively as:\n\n1) The state is embedded in $SO(2) \\times \\mathbb{R}^{2(1+L)}$, where:\n\n      * the retraction $\\varphi(.,.)$ is the $SO(2)$ exponential \n        map for orientation and the vector addition for robot \n        and landmark positions.\n\n      * the inverse retraction $\\varphi^{-1}(.,.)$ is the $SO(2)$ \n        logarithm for orientation and the vector subtraction for robot \n        and landmark positions.\n\n2) The state is embedded in $SE_{1+L}(2)$ with left multiplication, i.e.\n\n      * the retraction $\\varphi(.,.)$ is the $SE_{1+L}(2)$\n        exponential, where the state multiplies on the left the uncertainty\n        $\\boldsymbol{\\xi}$.\n\n      * the inverse retraction $\\varphi^{-1}(.,.)$ is the $SE_{1+L}\n        (2)$ logarithm. \n\n3) The state is embedded in $SE_{1+L}(2)$ with right multiplication, i.e.\n\n      * the retraction $\\varphi(.,.)$ is the $SE_{1+L}(2)$\n        exponential, where state multiplies on the right the uncertainty\n        $\\boldsymbol{\\xi}$.\n\n      * the inverse retraction $\\varphi^{-1}(.,.)$ is the $SE_{1+L}\n        (2)$ logarithm.\n\n      * it corresponds to the Invariant Extended Kalman Filter (IEKF)\n        recommended in  :cite:`barrauInvariant2017` that naturally leads to\n        resolve the consistency issue of traditional EKF-SLAM.\n\n<div class=\"alert alert-info\"><h4>Note</h4><p>$SE_{1+L}(2)$ exponential and logarithm are similar as their \n    $SE(2)$ counterpart, see `documentation <geometry>`.</p></div>\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "source": [
        "Import\n==============================================================================\nWe import a specific EKF for performing state augmentation and update with\ndifferent measurement dimension. Indeed, in 2D SLAM, unknown landmarks are\nprogressively added to the state the first time the landmark is observing. And\neach update consists of observed only visible landmarks. Both operations are\nalso handled in our advanced ``JUKF``.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "execution_count": null,
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      },
      "source": [
        "from ukfm.model.slam2d import EKF\nfrom ukfm import SO2, JUKF\nfrom ukfm import SLAM2D as MODEL\nimport numpy as np\nimport ukfm\nukfm.set_matplotlib_config()"
      ]
    },
    {
      "source": [
        "Simulation Setting\n==============================================================================\nWe compare the filters on a large number of Monte-Carlo runs.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "execution_count": null,
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      },
      "source": [
        "# Monte-Carlo runs\nN_mc = 100"
      ]
    },
    {
      "source": [
        "This script uses the ``SLAM2D`` model that requires sequence time and\nodometry frequency.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "execution_count": null,
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      },
      "source": [
        "# sequence time (s)\nT = 2500\n# odometry frequency (Hz)\nodo_freq = 1\n# create the model\nmodel = MODEL(T, odo_freq)"
      ]
    },
    {
      "source": [
        "The trajectory of the robot consists of turning at constant speed. The map\nwill be the same for all the simulation, where landmarks are constantly spaced\nalong the robot trajectory.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "execution_count": null,
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      },
      "source": [
        "# true speed of robot (m/s)\nv = 0.25\n# true angular velocity (rad/s)\ngyro = 1.5/180*np.pi\n# odometry noise standard deviation\nodo_std = np.array([0.05*v/np.sqrt(2),     # speed (v/m)\n                    0.05*v*np.sqrt(2)*2])  # angular speed (rad/s)\n# observation noise standard deviation (m)\nobs_std = 0.1"
      ]
    },
    {
      "source": [
        "Filter Design\n==============================================================================\nAdditionally to the three UKFs, we compare them to an EKF and an IEKF. The EKF\nhas the same uncertainty representation as the UKF with $SO(2) \\times\n\\mathbb{R}^{2(1+L)}$ uncertainty representation, whereas the IEKF has the\nsame uncertainty representation as the UKF with right $SE_{1+L}(2)$\nretraction.\n\nWe have five similar methods, but the UKF implementations slightly differs.\nIndeed, using our vanilla UKF works for all choice of retraction but is not\nadapted to the problem from a computationally point of view. And we spare\ncomputation only when Jacobian is known.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "execution_count": null,
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      },
      "source": [
        "# propagation noise covariance matrix\nQ = np.diag(odo_std**2)\n# measurement noise covariance matrix\nR = obs_std**2*np.eye(2)\n# initial uncertainty matrix\nP0 = np.zeros((3, 3))\n# sigma point parameter\nalpha = np.array([1e-3, 1e-3, 1e-3, 1e-3, 1e-3])\nred_idxs = np.array([0, 1, 2])  # indices corresponding to the robot state in P\naug_idxs = np.array([0, 1, 2])  # indices corresponding to the robot state in P"
      ]
    },
    {
      "source": [
        "We set variables for recording metrics before launching Monte-Carlo\nsimulations.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "execution_count": null,
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      },
      "source": [
        "ukf_err = np.zeros((N_mc, model.N, 3))\nleft_ukf_err = np.zeros_like(ukf_err)\nright_ukf_err = np.zeros_like(ukf_err)\niekf_err = np.zeros_like(ukf_err)\nekf_err = np.zeros_like(ukf_err)\n\nukf_nees = np.zeros((N_mc, model.N, 2))\nleft_ukf_nees = np.zeros_like(ukf_nees)\nright_ukf_nees = np.zeros_like(ukf_nees)\niekf_nees = np.zeros_like(ukf_nees)\nekf_nees = np.zeros_like(ukf_nees)"
      ]
    },
    {
      "source": [
        "Monte-Carlo Runs\n==============================================================================\nWe run the Monte-Carlo through a for loop.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "execution_count": null,
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      },
      "source": [
        "for n_mc in range(N_mc):\n    print(\"Monte-Carlo iteration(s): \" + str(n_mc+1) + \"/\" + str(N_mc))\n    # simulate true trajectory and noisy input\n    states, omegas, ldks = model.simu_f(odo_std, v, gyro)\n    # simulate landmark measurements\n    ys = model.simu_h(states, obs_std, ldks)\n    # initialize filter with true state\n    state0 = model.STATE(\n        Rot=states[0].Rot,\n        p=states[0].p,\n        p_l=np.zeros((0, 2))\n    )\n\n    ukf = JUKF(state0=state0, P0=P0, f=model.f, h=model.h, Q=Q, phi=model.phi,\n               alpha=alpha, red_phi=model.red_phi,\n               red_phi_inv=model.red_phi_inv, red_idxs=red_idxs,\n               up_phi=model.up_phi, up_idxs=np.arange(5), aug_z=model.aug_z,\n               aug_phi=model.aug_phi, aug_phi_inv=model.aug_phi_inv,\n               aug_idxs=aug_idxs, aug_q=2)\n    left_ukf = JUKF(state0=state0, P0=P0, f=model.f, h=model.h, Q=Q,\n                    phi=model.left_phi, alpha=alpha, red_phi=model.left_red_phi,\n                    red_phi_inv=model.left_red_phi_inv, red_idxs=red_idxs,\n                    up_phi=model.left_up_phi, up_idxs=np.arange(5),\n                    aug_z=model.aug_z, aug_phi=model.left_aug_phi,\n                    aug_phi_inv=model.left_aug_phi_inv, aug_idxs=aug_idxs,\n                    aug_q=2)\n    right_ukf = JUKF(state0=state0, P0=P0, f=model.f, h=model.h, Q=Q,\n                     phi=model.right_phi, alpha=alpha, aug_z=model.aug_z,\n                     red_phi=model.right_red_phi,\n                     red_phi_inv=model.right_red_phi_inv, red_idxs=red_idxs,\n                     up_phi=model.right_up_phi, up_idxs=np.arange(5),\n                     aug_phi=model.right_aug_phi,\n                     aug_phi_inv=model.right_aug_phi_inv,\n                     aug_idxs=aug_idxs, aug_q=2)\n    iekf = EKF(state0=state0, P0=P0, f=model.f, h=model.h, Q=Q,\n               phi=model.right_phi, z=model.z, aug_z=model.aug_z)\n    iekf.jacobian_propagation = iekf.iekf_FG_ana\n    iekf.H_num = iekf.iekf_jacobian_update\n    iekf.aug = iekf.iekf_augment\n\n    ekf = EKF(state0=state0, P0=P0, f=model.f, h=model.h, Q=Q,\n              phi=model.phi, z=model.z, aug_z=model.aug_z)\n    ekf.jacobian_propagation = ekf.ekf_FG_ana\n    ekf.H_num = ekf.ekf_jacobian_update\n    ekf.aug = ekf.ekf_augment\n\n    ukf_states = [state0]\n    left_ukf_states = [state0]\n    right_ukf_states = [state0]\n    iekf_states = [state0]\n    ekf_states = [state0]\n\n    ukf_Ps = [P0]\n    left_ukf_Ps = [P0]\n    right_ukf_Ps = [P0]\n    ekf_Ps = [P0]\n    iekf_Ps = [P0]\n\n    # indices of already observed landmarks\n    ukf_lmk = np.array([])\n\n    # The UKF proceeds as a standard Kalman filter with a for loop.\n    for n in range(1, model.N):\n        # propagation\n        ukf.propagation(omegas[n-1], model.dt)\n        left_ukf.red_d = left_ukf.P.shape[0]\n        left_ukf.red_idxs = np.arange(left_ukf.P.shape[0])\n        left_ukf.red_d = left_ukf.red_idxs.shape[0]\n        left_ukf.weights = left_ukf.WEIGHTS(left_ukf.red_d,\n                                            left_ukf.Q.shape[0], left_ukf.up_d,\n                                            left_ukf.aug_d, left_ukf.aug_q, alpha)\n        left_ukf.propagation(omegas[n-1], model.dt)\n        iekf.propagation(omegas[n-1], model.dt)\n        ekf.propagation(omegas[n-1], model.dt)\n        # propagation of right Jacobian\n        right_ukf.state_propagation(omegas[n-1], model.dt)\n        right_ukf.F = np.eye(right_ukf.P.shape[0])\n        right_ukf.red_d = right_ukf.P.shape[0]\n        right_ukf.red_idxs = np.arange(right_ukf.P.shape[0])\n        right_ukf.G_num(omegas[n-1], model.dt)\n        right_ukf.cov_propagation()\n        y_n = ys[n]\n        # observed landmarks\n        idxs = np.where(y_n[:, 2] >= 0)[0]\n        # update each landmark already in the filter\n        p_ls = ukf.state.p_l\n        left_p_ls = left_ukf.state.p_l\n        right_p_ls = right_ukf.state.p_l\n        iekf_p_ls = iekf.state.p_l\n        ekf_p_ls = ekf.state.p_l\n        for idx0 in idxs:\n            idx = np.where(ukf_lmk == y_n[idx0, 2])[0]\n            if idx.shape[0] == 0:\n                continue\n            # indices of the robot and observed landmark in P\n            up_idxs = np.hstack([0, 1, 2, 3+2*idx, 4+2*idx])\n            ukf.state.p_l = np.squeeze(p_ls[idx])\n            left_ukf.state.p_l = np.squeeze(left_p_ls[idx])\n            right_ukf.state.p_l = np.squeeze(right_p_ls[idx])\n            iekf.state.p_l = np.squeeze(iekf_p_ls[idx])\n            ekf.state.p_l = np.squeeze(ekf_p_ls[idx])\n            # compute observability matrices and residual\n            ukf.H_num(np.squeeze(y_n[idx0, :2]), up_idxs, R)\n            left_ukf.H_num(np.squeeze(y_n[idx0, :2]), up_idxs, R)\n            right_ukf.H_num(np.squeeze(y_n[idx0, :2]), up_idxs, R)\n            iekf.H_num(np.squeeze(y_n[idx0, :2]), up_idxs, R)\n            ekf.H_num(np.squeeze(y_n[idx0, :2]), up_idxs, R)\n        ukf.state.p_l = p_ls\n        left_ukf.state.p_l = left_p_ls\n        right_ukf.state.p_l = right_p_ls\n        iekf.state.p_l = iekf_p_ls\n        ekf.state.p_l = ekf_p_ls\n        # update only if some landmarks have been observed\n        if ukf.H.shape[0] > 0:\n            ukf.state_update()\n            left_ukf.state_update()\n            right_ukf.state_update()\n            iekf.state_update()\n            ekf.state_update()\n        # augment the state with new landmark\n        for idx0 in idxs:\n            idx = np.where(ukf_lmk == y_n[idx0, 2])[0]\n            if not idx.shape[0] == 0:\n                continue\n            # augment the landmark state\n            ukf_lmk = np.hstack([ukf_lmk, int(y_n[idx0, 2])])\n            # indices of the new landmark\n            idx = ukf_lmk.shape[0] - 1\n            # new landmark position\n            p_l = np.expand_dims(\n                ukf.state.p + ukf.state.Rot.dot(y_n[idx0, :2]), 0)\n            left_p_l = np.expand_dims(\n                left_ukf.state.p + left_ukf.state.Rot.dot(y_n[idx0, :2]), 0)\n            right_p_l = np.expand_dims(\n                right_ukf.state.p + right_ukf.state.Rot.dot(y_n[idx0, :2]), 0)\n            iekf_p_l = np.expand_dims(\n                iekf.state.p + iekf.state.Rot.dot(y_n[idx0, :2]), 0)\n            ekf_p_l = np.expand_dims(\n                ekf.state.p + ekf.state.Rot.dot(y_n[idx0, :2]), 0)\n            p_ls = np.vstack([ukf.state.p_l, p_l])\n            left_p_ls = np.vstack([left_ukf.state.p_l, left_p_l])\n            right_p_ls = np.vstack([right_ukf.state.p_l, right_p_l])\n            iekf_p_ls = np.vstack([iekf.state.p_l, iekf_p_l])\n            ekf_p_ls = np.vstack([ekf.state.p_l, ekf_p_l])\n            ukf.state.p_l = p_l\n            left_ukf.state.p_l = left_p_l\n            right_ukf.state.p_l = right_p_l\n            iekf.state.p_l = iekf_p_l\n            ekf.state.p_l = ekf_p_l\n\n            # get Jacobian and then covariance\n            R_n = obs_std ** 2 * np.eye(2)\n            ukf.aug(y_n[idx0, :2], aug_idxs, R)\n            left_ukf.aug(y_n[idx0, :2], aug_idxs, R)\n            right_ukf.aug(y_n[idx0, :2], aug_idxs, R)\n            iekf.aug(y_n[idx0, :2], aug_idxs, R)\n            ekf.aug(y_n[idx0, :2], aug_idxs, R)\n            ukf.state.p_l = p_ls\n            left_ukf.state.p_l = left_p_ls\n            right_ukf.state.p_l = right_p_ls\n            iekf.state.p_l = iekf_p_ls\n            ekf.state.p_l = ekf_p_ls\n\n        # save estimates\n        ukf_states.append(ukf.state)\n        left_ukf_states.append(left_ukf.state)\n        right_ukf_states.append(right_ukf.state)\n        iekf_states.append(iekf.state)\n        ekf_states.append(ekf.state)\n\n        ukf_Ps.append(ukf.P)\n        left_ukf_Ps.append(left_ukf.P)\n        right_ukf_Ps.append(right_ukf.P)\n        iekf_Ps.append(iekf.P)\n        ekf_Ps.append(ekf.P)\n\n    #\u00a0get state trajectory\n    Rots, ps = model.get_states(states, model.N)\n    ukf_Rots, ukf_ps = model.get_states(ukf_states, model.N)\n    left_ukf_Rots, left_ukf_ps = model.get_states(left_ukf_states, model.N)\n    right_ukf_Rots, right_ukf_ps = model.get_states(right_ukf_states, model.N)\n    iekf_Rots, iekf_ps = model.get_states(iekf_states, model.N)\n    ekf_Rots, ekf_ps = model.get_states(ekf_states, model.N)\n\n    # record errors\n    ukf_err[n_mc] = model.errors(Rots, ukf_Rots, ps, ukf_ps)\n    left_ukf_err[n_mc] = model.errors(Rots, left_ukf_Rots, ps, left_ukf_ps)\n    right_ukf_err[n_mc] = model.errors(Rots, right_ukf_Rots, ps, right_ukf_ps)\n    iekf_err[n_mc] = model.errors(Rots, iekf_Rots, ps, iekf_ps)\n    ekf_err[n_mc] = model.errors(Rots, ekf_Rots, ps, ekf_ps)\n\n    # record NEES\n    ukf_nees[n_mc] = model.nees(ukf_err[n_mc], ukf_Ps, ukf_Rots, ukf_ps, 'STD')\n    left_ukf_nees[n_mc] = model.nees(left_ukf_err[n_mc], left_ukf_Ps,\n                                     left_ukf_Rots, left_ukf_ps, 'LEFT')\n    right_ukf_nees[n_mc] = model.nees(right_ukf_err[n_mc], right_ukf_Ps,\n                                      right_ukf_Rots, right_ukf_ps, 'RIGHT')\n    iekf_nees[n_mc] = model.nees(iekf_err[n_mc], iekf_Ps, iekf_Rots, iekf_ps,\n                                 'RIGHT')\n    ekf_nees[n_mc] = model.nees(ekf_err[n_mc], ekf_Ps, ekf_Rots, ekf_ps, 'STD')"
      ]
    },
    {
      "source": [
        "Results\n------------------------------------------------------------------------------\nWe first visualize the results for the last run, and then plot the orientation\nand position errors averaged over Monte-Carlo.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "execution_count": null,
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      },
      "source": [
        "#\u00a0get state\nRots, ps = model.get_states(states, model.N)\nukf_Rots, ukf_ps = model.get_states(ukf_states,  model.N)\nleft_ukf_Rots, left_ukf_ps = model.get_states(left_ukf_states,  model.N)\nright_ukf_Rots, right_ukf_ps = model.get_states(right_ukf_states,  model.N)\niekf_Rots, iekf_ps = model.get_states(iekf_states,  model.N)\nekf_Rots, ekf_ps = model.get_states(ekf_states,  model.N)\n\nukf_err, left_ukf_err, right_ukf_err, iekf_err, ekf_err = model.benchmark_plot(\n    ukf_err, left_ukf_err, right_ukf_err, iekf_err, ekf_err, ps, ukf_ps,\n    left_ukf_ps, right_ukf_ps, ekf_ps, iekf_ps)"
      ]
    },
    {
      "source": [
        "We then compute the Root Mean Squared Error (RMSE) for each method both for\nthe orientation and the position.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "execution_count": null,
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      },
      "source": [
        "model.benchmark_print(ukf_err, left_ukf_err, right_ukf_err, iekf_err, ekf_err)"
      ]
    },
    {
      "source": [
        "Right UKF and IEKF outperform the remaining filters.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "source": [
        "We now compare the filters in term of consistency (NEES).\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "execution_count": null,
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      },
      "source": [
        "model.nees_print(ukf_nees, left_ukf_nees, right_ukf_nees, iekf_nees, ekf_nees)"
      ]
    },
    {
      "source": [
        "The right UKF and the IEKF obtain similar NEES and are the more consistent\nfilters, whereas the remaining filter have their NEES increasing.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "source": [
        "**Which filter is the most accurate ?** The **right UKF** and the **IEKF** are\nthe best both in term of accuracy and consistency.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "source": [
        "Conclusion\n==============================================================================\nThis script compares different algorithms for 2D robot SLAM. The **right UKF**\nand the **IEKF** are the more accurate filters. They are also consistent along\nall the trajectory.\n\nYou can now:\n\n- compare the filters in different scenarios. UKF and their (I)EKF\n  counterparts may obtain different results when noise is inflated.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    }
  ],
  "nbformat_minor": 0,
  "metadata": {
    "kernelspec": {
      "name": "python3",
      "language": "python",
      "display_name": "Python 3"
    },
    "language_info": {
      "version": "3.5.2",
      "codemirror_mode": {
        "version": 3,
        "name": "ipython"
      },
      "pygments_lexer": "ipython3",
      "name": "python",
      "file_extension": ".py",
      "nbconvert_exporter": "python",
      "mimetype": "text/x-python"
    }
  },
  "nbformat": 4
}